package com.yxp.hard.astar;

import java.util.LinkedList;
import java.util.List;

public class AStartTest {

    private static byte[][] mapData = {
            {0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,},
            {0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0,},
            {0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0,},
            {0, 0, 1, 0, 1, 1, 0, 0, 0, 0, 1,},
            {0, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0,},
            {0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0,},
            {0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0,},
            {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,},
            {0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,},
    };

    public static void main(String[] args) {
        List<Pair<Integer, Integer>> path = findPath(0, 5, 3, 10);
        System.out.println(path);
    }

    public static List<Pair<Integer, Integer>> findPath(int fromRow, int fromColumn, int toRow, int toColumn) {

        int[][] distance = new int[20][20];
        for (int j = 0; j < 20; j++) {
            for (int i = 0; i < 20; i++) {
                distance[j][i] = Integer.MAX_VALUE;
            }
        }

        SortLinkedList priorityList = new SortLinkedList();
        AStarNode startNode = new AStarNode(fromRow, fromColumn, mapData);
        AStarNode goalNode = new AStarNode(toRow, toColumn, mapData);
        priorityList.add(new AStarNode(fromRow, fromColumn, mapData));

        AStarNode lastNode = new AStarNode(toRow, toColumn, mapData);
        lastNode.searchParent = null;
        lastNode.G = -1;// 做标记判断
        priorityList.add(lastNode);

        // 开始节点
        startNode.searchParent = null;
        priorityList.add(startNode);

        while (!priorityList.isEmpty()) {
            System.out.println("====================================");
            AStarNode curNode = priorityList.removeFirst();
            System.out.println("curNode：" + curNode);
            if (curNode.G == -1) {
                break;
            }
            if (curNode.equals(goalNode)) {
                return construct(curNode);
            } else {
                List<AStarNode> neighbors = curNode.getNeighbors(mapData);
                System.out.println("neighbors：" + neighbors);
                for (AStarNode n : curNode.getNeighbors(mapData)) {
                    if (n.data != 0) {
                        int h = curNode.calculateG(n);
                        int d = distance[n.getRow()][n.getColumn()];
                        if (h < d) {
                            distance[n.getRow()][n.getColumn()] = h;
                            n.searchParent = curNode;
                            n.calculateF(goalNode);
                            priorityList.add(n);
                            System.out.println("add n ：" + n);
                        }
                    }
                }
            }
        }
        return null;
    }

    // 回溯路径
    private static LinkedList<Pair<Integer, Integer>> construct(AStarNode node) {
        LinkedList<Pair<Integer, Integer>> path = new LinkedList<>();
        while (node != null) {
            path.addFirst(new Pair<>(node.getRow(), node.getColumn()));
            node = node.searchParent;
        }
        return path;
    }
}
